#include <no_control.h>

namespace controllers
{
	No_Control::No_Control() {}
	No_Control::~No_Control() {}

	bool No_Control::init(std::vector<HHD> &hHD)
	{
		_hHD = hHD;

		return true;
	}

	void No_Control::starting(const boost::posix_time::ptime& time)
	{
		
	}

	void No_Control::update(const boost::posix_time::ptime& time, const boost::posix_time::time_duration& period)
	{
		for (int i = 0; i < _hHD.size(); i++)
		{
			hdMakeCurrentDevice(_hHD[i]);

			/* Begin haptics frame.  ( In general, all state-related haptics calls
			should be made within a frame. ) */
			//hdBeginFrame(_hHD[i]);

			/* Get the current position of the device. */
			hdGetDoublev(HD_CURRENT_POSITION, hdu_position[i]);
			/* Get the current velocity of the device. */
			hdGetDoublev(HD_CURRENT_VELOCITY, hdu_velocity[i]);

			for (int j = 0; j < 3; j++)
			{
				position[i](j) = hdu_position[i][j];
				velocity[i](j) = hdu_velocity[i][j];
			}

			force[i].setZero();
			force_p[i].setZero();
			force_d[i].setZero();
		}

		memset(hdu_force, 0, 2*sizeof(hduVector3Dd));

		/* Write your code HERE! */

		for (int i = 0; i < _hHD.size(); i++)
		{
			hdMakeCurrentDevice(_hHD[i]);

			/* Send the force to the device. */
			hdSetDoublev(HD_CURRENT_FORCE, hdu_force[i]);

			/* End haptics frame. */
			//hdEndFrame(_hHD[i]);

			/* Check for errors and abort the callback if a scheduler error
			is detected. */
			if (HD_DEVICE_ERROR(error = hdGetError()))
			{
				hduPrintError(stderr, &error, 
					"Error detected while rendering gravity well\n");

				//if (hduIsSchedulerError(&error))
				//{
				//	return HD_CALLBACK_DONE;
				//}
			}
		}

		/* Signify that the callback should continue running, i.e. that
		it will be called again the next scheduler tick. */
		//return HD_CALLBACK_CONTINUE;
	}

	void No_Control::command()
	{
		
	}

	void No_Control::stopping(const boost::posix_time::ptime& time)
	{
		
	}
}
